/*******************************************************
 * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science
 *and Technology
 *
 * This file is part of VINS.
 *
 * Licensed under the GNU General Public License v3.0;
 * you may not use this file except in compliance with the License.
 *
 * Author: Qin Tong (qintonguav@gmail.com)
 *******************************************************/

#pragma once

#include <ceres/ceres.h>
#include <ros/assert.h>
#include <Eigen/Dense>

#include "lci_slam/vilens/utility/utility.h"

class PriorPoseFactor : public ceres::SizedCostFunction<6, 7> {
public:
    PriorPoseFactor(const Eigen::Vector3d &_P, const Eigen::Quaterniond &_Q) {
        init_P = _P;
        init_Q = _Q;
        sqrt_info = 1000 * Eigen::Matrix<double, 6, 6>::Identity();
    }
    virtual bool Evaluate(double const *const *parameters, double *residuals,
                          double **jacobians) const {
        Eigen::Vector3d P(parameters[0][0], parameters[0][1], parameters[0][2]);
        Eigen::Quaterniond Q(parameters[0][6], parameters[0][3],
                             parameters[0][4], parameters[0][5]);

        Eigen::Map<Eigen::Matrix<double, 6, 1>> residual(residuals);
        residual.block<3, 1>(0, 0) = P - init_P;
        residual.block<3, 1>(3, 0) = 2 * (init_Q.inverse() * Q).vec();
        residual = sqrt_info * residual;

        if (jacobians) {
            if (jacobians[0]) {
                Eigen::Map<Eigen::Matrix<double, 6, 7, Eigen::RowMajor>>
                    jacobian_pose(jacobians[0]);
                jacobian_pose.setZero();
                jacobian_pose.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
                jacobian_pose.block<3, 3>(3, 3) =
                    Utility::Qleft(init_Q.inverse() * Q)
                        .bottomRightCorner<3, 3>();
                jacobian_pose = sqrt_info * jacobian_pose;
            }
        }
        return true;
    }

    Eigen::Vector3d init_P;
    Eigen::Quaterniond init_Q;
    Eigen::Matrix<double, 6, 6> sqrt_info;
};
